//
// Created by dwx on 2024/4/3.
//
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "dwt_stm32_delay.h"
#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "stdio.h"


uint8_t SystemLock = 0;
uint8_t LCDLock = 0;
uint8_t XbeeWork = 0;
uint16_t  BatGetVaule = 0;
//LED0任务函数
uint8_t global_percent = 60,global_percentold = 0,global_percentDis=0;
//static uint16_t bat_adc_value[RAW_NUM] = {0};

uint8_t  *RobotStatus[11] = {"Default","Calibra","Origin","Paused","Stop","Unload","Command","Working","Shutdown","Recover","xxxxx"};
float RobotInfo[12] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
float RobotInfo_old[12] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
uint8_t DataChangeFlag = 1;
uint8_t XbeeRecTempBuf[1024];
uint8_t  AxisData[5] = {0,0,0,0,1},AxisIndex = 0;
float AxisSendData[3] = {0.001f,0.001f,0.001f};
uint8_t SystemWorkLock = 0;
uint8_t  SaveRobotSta = 0; // 0 未知�????????1 急停 2 指令
uint8_t  JointMode = 1,CoordinateMode = 1,CoordinateModeOld = 0xff,JointModeOld = 0xff;
uint8_t  ContralMode = 1;
uint8_t  robot_working = 10,robot_working0ld = 0xff;
uint8_t  Jointnum = 1,Coordinatenum = 1,JointnumOld = 0xff,CoordinatenumOld = 0xff;
uint8_t  RobotSpeedMode = 0;
uint8_t  RobotSpeedIndex = 0;
uint8_t  RobotSpeedData[3] = {0,5,0};
uint8_t  RobotSpeedChange = 0;
uint16_t  RobotSpeedValue = 0;
uint8_t  RobotAxisLevel[3] = {0,0,1},RobotGestureLevel[5] = {0,0,0,0,1},RobotJointLevel[3] = {0,5,1};
uint8_t  RobotAxisIndex = 0,RobotGestureIndex = 0,RobotJointIndex = 0;
uint8_t SetMyFlg[3] = {0,0,0};
float RobotAxisLevelData = 0.0f,RobotGestureLevelData= 0.0f,RobotJointLevelData=0.0f;
float  RobotSendData[12] = {0.001f,0.001f,0.001f,0.01f,0.00f,0.10f,0.01f,0.01f,0.01f,0.01f,0.01f,0.01f};
uint8_t RobotChooseSta[6] = {1,1,1,1,1,1};
//uint8_t RobotDataSendCmd = 1;
uint8_t RobotDataStatusS[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
uint8_t  RobotDataStatusFlag = 1;
uint8_t  RobotDataSendIndex = 0;
uint8_t  SendDataToMain = 0;
uint8_t  SendCmdClass = 0;
uint8_t  RobotJointDisPlay = 1;
uint8_t  RobotJointSendCmd = 0;
uint8_t  RobotReportLock = 0;
float OriginAngle[6] = {0.0f,0.0f,90.0f,0.0f,0.0f,0.0f};
float ChangeAnale[6] = {0.0f,0.0f,90.0f,0.0f,0.0f,0.0f};
float AxisOrgginData[6] = {420.0000f,0.0016f,373.0014f,-0.0002f,90.0000f,0.0000f};
float ChangeAxisOrgginData[6] = {420.0000f,0.0016f,373.0014f,-0.0002f,90.0000f,0.0000f};
uint8_t  SendFlah = 1;


uint8_t lcdbuff[500];
void lcd_printf(const char *format, ...) {
    vPortEnterCritical();
    va_list args;
    uint32_t length;
    va_start(args, format);
    length = vsnprintf((char *) lcdbuff, sizeof(lcdbuff), (char *) format, args);
    va_end(args);
    HAL_UART_Transmit(&huart2,lcdbuff,length,HAL_MAX_DELAY);
    vPortExitCritical();
}
uint8_t  GetDataBitNum(int32_t data){
    uint8_t  bit = 0;
    do{
        bit+=1;
        data/=10;
    }while(data!=0);
    return bit;
}
void ClearRobotSendStatus(void){
    memset(RobotDataStatusS,0,sizeof (RobotDataStatusS));
}
void SetSendRobotStatus(uint8_t cmd){
    switch (cmd) {
        case 0:
            lcd_printf("BOXF(279,340,319,379,61);CIR(289,350,10,0);\r\n"); delay_ms(LCDRufusTime);
            break;
        case 1:
            lcd_printf("BOXF(279,340,319,379,61);PL(289,341,289,365,0);PL(289,341,280,351,0);PL(289,341,298,351,0);\r\n"); delay_ms(LCDRufusTime);
            break;
        case 2:
            lcd_printf("BOXF(279,340,319,379,61);PL(279,350,285,360,0);PL(285,360,309,341,0);\r\n"); delay_ms(LCDRufusTime);
            break;
    }
}
void Menu_Config(void){
    delay_ms(500);
    lcd_printf("BL(100);CLR(61);DR(0);DC32(0,1,'X:',0);DC32(161,1,'[%s]',0);\r\n",RobotStatus[robot_working]);
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(0,1,'X:',0);\r\n");
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(40,41,'%0.2f',0);\r\n",RobotInfo[0]);
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(0,81,'Y:',0);\r\n");
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(40,121,'%0.2f',0);\r\n",RobotInfo[1]);
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(0,161,'Z:',0);\r\n");
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(40,201,'%0.2f',0);\r\n",RobotInfo[2]);
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(0,241,'====================',0);\r\n");
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(0,281,'[X Y Z] X ',0);\r\n");
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(160,281,'%0.2fcm',0);\r\n",AxisData[0]);
    delay_ms(LCDRufusTime);
    if(AxisSendData[0]<0){
        lcd_printf("DC32(10,341,'%0.2fcm',0);\r\n",fabs(AxisSendData[0]));
    }
    else {
        lcd_printf("DC32(10,341,'%0.2fcm',0);\r\n",fabs(AxisSendData[0]));
    }
    delay_ms(LCDRufusTime);
    //lcd_printf("DC32(10,361,'Speed:%03d',0);\r\n",SpeedValue);
    lcd_printf("DC32(10,391,'Speed:',0);\r\n");
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(110,391,'%d',0);\r\n",RobotSpeedData[0]); delay_ms(LCDRufusTime);
    lcd_printf("DC32(125,391,'%d',0);\r\n",RobotSpeedData[1]); delay_ms(LCDRufusTime);
    lcd_printf("DC32(140,391,'%d',0);\r\n",RobotSpeedData[2]);
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(180,441,'Bat:%d%%',0);\r\n",global_percent);
    delay_ms(LCDRufusTime);
    lcd_printf("DC32(10,441,'Ver:V%d',0);\r\n",SystemVersion);
    delay_ms(LCDRufusTime);
}
int16_t get_signed_diff(int i, int j) {
    uint16_t diff = j - i;
    if (diff >= (1U << 15)) {
        diff = diff + (1U << 16) - 1;
        if (diff >= (1U << 15)) {
            diff = diff - (1U << 16) + 1;
        }
    }
    return (int16_t)diff;
}
uint8_t CRCData(uint8_t *buf,uint16_t len){
    uint8_t checksum = 0;
    for(int i = 0;i<len;i++){
        checksum += buf[i];
    }
    return checksum;
}
void ChenckRobotInfo(void){
    vPortEnterCritical();
    static uint8_t sendbuff[7];
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    sendbuff[0] = 0xfb;
    sendbuff[1] = 0xa0;
    sendbuff[2] = sizeof(sendbuff);
    sendbuff[3] = 0x00;
    sendbuff[4] = 0x00;
    sendbuff[5] = 0x00;
    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetRobotPause(void){
    static uint8_t sendbuff[12];
    vPortEnterCritical();
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    sendbuff[0] = 0xfc;
    sendbuff[1] = 0xc1;
    sendbuff[2] = sizeof(sendbuff);
    sendbuff[3] = 0x00;
    sendbuff[4] = 0x00;
    sendbuff[5] = 0x04;
    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetRobotWork(void){
    vPortEnterCritical();
    static uint8_t sendbuff[12];
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    sendbuff[0] = 0xfc;
    sendbuff[1] = 0xc1;
    sendbuff[2] = sizeof(sendbuff);
    sendbuff[3] = 0x00;
    sendbuff[4] = 0x00;
    sendbuff[5] = 0x07;
    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetRobotTest(uint8_t cmd){
    vPortEnterCritical();
    static uint8_t sendbuff[12];
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    sendbuff[0] = 0xfc;
    sendbuff[1] = 0xc1;
    sendbuff[2] = sizeof(sendbuff);
    sendbuff[3] = 0x00;
    sendbuff[4] = 0x00;
    sendbuff[5] = cmd;
    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetRobotFromAngle(uint8_t Jointnum,float angle,float system_speed){
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    vPortEnterCritical();
    static uint8_t sendbuff[32];
    static uint8_t  buff[4],speedbuf[4];
    memcpy(buff,&angle,sizeof (angle));
    memcpy(speedbuf,&system_speed,sizeof(system_speed));
    sendbuff[0] = 0xfb;
    sendbuff[1] = 0xa2;
    sendbuff[2] = sizeof(sendbuff);
    sendbuff[3] = 0x00;
    sendbuff[4] = 0x02;
    for(int i = 0;i<4;i++){
        sendbuff[i+5] = 0x00;
    }
    sendbuff[9] = Jointnum;
    sendbuff[10] = Jointnum;
    for(int i = 0;i<4;i++){
        sendbuff[i+11] = buff[i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+15] = speedbuf[i];

    }
    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetRobotFromAngleLosr(float Angle1,float angle2,float angle3,float angle4,float angle5,float angle6,float system_speed){
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    static uint8_t sendbuff[45];
    static uint8_t  buff[6][4],speedbuf[4];
    static float OriginData[6] = {0.0f,0.0f,90.0f,0.0f,0.0f,0.0f};
    vPortEnterCritical();
    if(Angle1 == 0.0f && angle2 == 0.0f && angle3 == 0.0f && angle4 == 0.0f && angle5 == 0.0f && angle6 == 0.0f){
        for(int i = 0;i<6;i++){
            memcpy(buff[i],&OriginData[i],sizeof(OriginData[0]));
        }
    }
    else {
        memcpy(buff[0],&Angle1,sizeof(float));
        memcpy(buff[1],&angle2,sizeof(float));
        memcpy(buff[2],&angle3,sizeof(float));
        memcpy(buff[3],&angle4,sizeof(float));
        memcpy(buff[4],&angle5,sizeof(float));
        memcpy(buff[5],&angle6,sizeof(float));
    }
    memcpy(speedbuf,&system_speed,sizeof(system_speed));
    sendbuff[0] = 0xfb;
    sendbuff[1] = 0xa2;
    sendbuff[2] = sizeof(sendbuff);
    sendbuff[3] = 0x00;
    sendbuff[4] = 0x01;
    for(int i = 0;i<4;i++){
        sendbuff[i+5] = 0x00;
    }
    sendbuff[9] = 1;
    sendbuff[10] = 6;
    for(int i = 0;i<4;i++){
        sendbuff[i+11] = buff[0][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+15] = buff[1][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+19] = buff[2][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+23] = buff[3][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+27] = buff[4][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+31] = buff[5][i];
    }


    for(int i = 0;i<4;i++){
        sendbuff[i+35] = speedbuf[i];

    }
    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetMotorOnCoordinate(float X_data,float Y_data,float  Z_data,float A_data,float B_data,float C_data,float Speed){
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    static uint8_t sendbuff[40] ;
    uint8_t buff[7][4];
    uint8_t  crc_sum = 0;
    vPortEnterCritical();
    memcpy(buff[0],&X_data,sizeof (float ));
    memcpy(buff[1],&Y_data,sizeof (float ));
    memcpy(buff[2],&Z_data,sizeof (float ));
    memcpy(buff[3],&A_data,sizeof (float ));
    memcpy(buff[4],&B_data,sizeof (float ));
    memcpy(buff[5],&C_data,sizeof (float ));
    memcpy(buff[6],&Speed,sizeof (float ));
    sendbuff[0]=0xfb;
    sendbuff[1]=0xa3;
    sendbuff[2]=sizeof (sendbuff);
    sendbuff[4]=0X02;//1???????é??? 2 ?????–é??
    for(int i = 0;i<4;i++){
        sendbuff[i+9] = buff[0][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+13] = buff[1][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+17] = buff[2][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+21] = buff[3][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+25] = buff[4][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+29] = buff[5][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+33] = buff[6][i];
    }

    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void SetMotorOnCoordinateLost(float X_data,float Y_data,float  Z_data,float A_data,float B_data,float C_data,float Speed){
#ifdef ModeToMore
    uint8_t Changrbuf[1024];
    uint8_t SendLen = 0;
#endif
    static uint8_t sendbuff[40] ;
    uint8_t buff[7][4];
    uint8_t  crc_sum = 0;
    static float OriginData[6] = {420.0000f,0.0016f,373.0014f,-0.0002f,90.0000f,0.0000f};
    static float Test[6] ;
    vPortEnterCritical();
    if(X_data == 0.0f && Y_data == 0.0f && Z_data == 0.0f && A_data == 0.0f && B_data == 0.0f && C_data == 0.0f){
        Test[0] = OriginData[0]/1000;
        Test[1] = OriginData[1]/1000;
        Test[2] = OriginData[2]/1000;
        Test[3] = OriginData[3];
        Test[4] = OriginData[4];
        Test[5] = OriginData[5];
        memcpy(buff[0],&Test[0],sizeof (float ));
        memcpy(buff[1],&Test[1],sizeof (float ));
        memcpy(buff[2],&Test[2],sizeof (float ));
        memcpy(buff[3],&Test[3],sizeof (float ));
        memcpy(buff[4],&Test[4],sizeof (float ));
        memcpy(buff[5],&Test[5],sizeof (float ));


    }
    else {
        Test[0] = X_data;
        Test[1] = Y_data;
        Test[2] = Z_data;
        Test[3] = A_data;
        Test[4] = B_data;
        Test[5] = C_data;

        memcpy(buff[0],&Test[0],sizeof (float ));
        memcpy(buff[1],&Test[1],sizeof (float ));
        memcpy(buff[2],&Test[2],sizeof (float ));
        memcpy(buff[3],&Test[3],sizeof (float ));
        memcpy(buff[4],&Test[4],sizeof (float ));
        memcpy(buff[5],&Test[5],sizeof (float ));

    }
    memcpy(buff[6],&Speed,sizeof (float ));
    sendbuff[0]=0xfb;
    sendbuff[1]=0xa3;
    sendbuff[2]=sizeof (sendbuff);
    sendbuff[4]=0X01;//1???????é??? 2 ?????–é??
    for(int i = 0;i<4;i++){
        sendbuff[i+9] = buff[0][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+13] = buff[1][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+17] = buff[2][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+21] = buff[3][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+25] = buff[4][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+29] = buff[5][i];
    }
    for(int i = 0;i<4;i++){
        sendbuff[i+33] = buff[6][i];
    }

    sendbuff[sendbuff[2]-1] = CRCData(sendbuff,sendbuff[2]-1);
#ifndef ModeToMore
    HAL_UART_Transmit(&huart3,sendbuff,sendbuff[2],HAL_MAX_DELAY);
#else
    ChangeDataToAPI(Changrbuf,sendbuff,sendbuff[2]);
    SendLen = (Changrbuf[1]<<8|Changrbuf[2])+4;
    Xbeesend(Changrbuf,SendLen);
    memset(Changrbuf,0,sizeof (Changrbuf));
    SendLen = 0;
#endif
    vPortExitCritical();
}
void lcdentry(void *arg){

    uint8_t lcdflag = 0;
    static uint32_t DataTemp = 0;
    while(1){
        if(SystemLock == 1 &&LCDLock == 0){
            lcd_printf("BL(255);\r\n");
        }
        else if(SystemLock == 1 && LCDLock == 1){
            if(lcdflag == 0){
                Menu_Config();
                lcdflag = 1;
            }
            else if(lcdflag == 1){
                if (ContralMode == 0) {
                    RobotJointDisPlay = 1;
                    if (CoordinateMode == 1) {
                        if (RobotAxisIndex == 0) {
                            if (SetMyFlg[0] == 0x01) {
                                SetMyFlg[0] = 0x00;
                                CoordinateModeOld = 0xff;
                                //RobotDataSendCmd = 1;
                            }
                        } else if (RobotAxisIndex > 0) {
                            SetMyFlg[0] = 0x01;
                            switch (RobotAxisIndex) {

                                case 1:
                                    lcd_printf("DC32(161,281,'%d',0);\r\n", RobotAxisLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',0);\r\n", RobotAxisLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(203,280,218,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'%d',1);\r\n", RobotAxisLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(220,281,'cm',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotAxisLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotAxisLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(73,340,87,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'%d',0);\r\n", RobotAxisLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(90,341,'cm',0);\r\n");
                                    break;
                                case 2:
                                    lcd_printf("DC32(161,281,'%d',0);\r\n", RobotAxisLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(174,280,188,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',1);\r\n", RobotAxisLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'%d',0);\r\n", RobotAxisLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(220,281,'cm',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotAxisLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(44,340,58,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotAxisLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'%d',0);\r\n", RobotAxisLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(90,341,'cm',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    break;
                                case 3:
                                    lcd_printf("BOXF(160,280,174,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(161,281,'%d',1);\r\n", RobotAxisLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',0);\r\n", RobotAxisLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'%d',0);\r\n", RobotAxisLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(220,281,'cm',0);\r\n");
                                    delay_ms(LCDRufusTime);


                                    lcd_printf("BOXF(30,340,44,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotAxisLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotAxisLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'%d',0);\r\n", RobotAxisLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(90,341,'cm',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    break;
                            }
                        }
                    } else if (CoordinateMode == 2) {
                        if (RobotGestureIndex == 0) {
                            if (SetMyFlg[1] == 0x01) {
                                SetMyFlg[1] = 0x00;
                                CoordinateModeOld = 0xff;
                                //RobotDataSendCmd = 1;
                            }

                        } else if (RobotGestureIndex > 0) {
                            SetMyFlg[1] = 0x01;
                            switch (RobotGestureIndex) {
                                case 1:
                                    lcd_printf("DC32(161,281,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(219,281,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(233,280,248,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(234,281,'%d',1);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(255,285,3,0);\r\n");
                                    delay_ms(LCDRufusTime);

                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(89,341,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(103,340,117,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(104,341,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(125,345,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    break;
                                case 2:

                                    lcd_printf("DC32(161,281,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(218,280,232,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(219,281,'%d',1);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(234,281,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(255,285,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(88,340,103,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(89,341,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(104,341,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(125,345,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    break;
                                case 3:

                                    lcd_printf("DC32(161,281,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(188,280,203,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'%d',1);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(219,281,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(234,281,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(255,285,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(58,340,73,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(89,341,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(104,341,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(125,345,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    break;
                                case 4:

                                    lcd_printf("DC32(161,281,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(174,280,188,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',1);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(219,281,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(234,281,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(255,285,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(44,340,58,379,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(89,341,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(104,341,'%d',0);\r\n", RobotGestureLevel[4]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(125,345,3,0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    break;
                                case 5:
                                    lcd_printf("BOXF(160,280,174,339,61);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(161,281,'%d',1);\r\n", RobotGestureLevel[0]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(175,281,'%d',0);\r\n", RobotGestureLevel[1]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(189,281,'%d',0);\r\n", RobotGestureLevel[2]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(204,281,'.',0);\r\n");
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(219,281,'%d',0);\r\n", RobotGestureLevel[3]);
                                    delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(234,281,'%d',0);\r\n", RobotGestureLevel[4]); delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(255,285,3,0);\r\n"); delay_ms(LCDRufusTime);

                                    lcd_printf("BOXF(30,340,44,379,61);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(31,341,'%d',0);\r\n", RobotGestureLevel[0]); delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(45,341,'%d',0);\r\n", RobotGestureLevel[1]); delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(59,341,'%d',0);\r\n", RobotGestureLevel[2]); delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(74,341,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(89,341,'%d',0);\r\n", RobotGestureLevel[3]); delay_ms(LCDRufusTime);
                                    lcd_printf("DC32(104,341,'%d',0);\r\n", RobotGestureLevel[4]); delay_ms(LCDRufusTime);
                                    lcd_printf("CIR(125,345,3,0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                            }
                        }
                    }
                    if (CoordinateMode != CoordinateModeOld) {
                        JointModeOld = 0xff;
                        if (CoordinateMode == 1) {
                            lcd_printf("BOXF(0,0,140,39,61);DC32(1,1,'X:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,80,140,119,61);DC32(1,81,'Y:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,160,140,199,61);DC32(1,161,'Z:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,280,159,319,61);DC32(1,281,'[X Y Z] X',0);\r\n"); delay_ms(LCDRufusTime);
                            if (RobotAxisIndex  == 0 ) {
                                lcd_printf("BOXF(160,280,319,319,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(161,281,'%d',0);\r\n", RobotAxisLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(175,281,'%d',0);\r\n", RobotAxisLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(189,281,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(204,281,'%d',0);\r\n", RobotAxisLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(220,281,'cm',0);\r\n"); delay_ms(LCDRufusTime);
                                RobotAxisLevelData = RobotAxisLevel[0] * 10.0f + RobotAxisLevel[1] * 1.0f +
                                                     RobotAxisLevel[2] * 0.1f ;

                                lcd_printf("BOXF(0,320,275,379,61);\r\n");
                                delay_ms(LCDRufusTime);
                                lcd_printf("DC32(31,341,'%d',0);\r\n", RobotAxisLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(45,341,'%d',0);\r\n", RobotAxisLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(59,341,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(74,341,'%d',0);\r\n", RobotAxisLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(90,341,'cm',0);\r\n"); delay_ms(LCDRufusTime);
                            }
                        } else if (CoordinateMode == 2) {
                            lcd_printf("BOXF(0,0,140,39,61);DC32(1,1,'A:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,80,140,119,61);DC32(1,81,'B:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,160,140,199,61);DC32(1,161,'C:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,280,159,319,61);DC32(1,281,'[A B C] X ',0);\r\n"); delay_ms(LCDRufusTime);
                            if (RobotGestureIndex == 0 ) {
                                lcd_printf("BOXF(160,280,319,319,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(161,281,'%d',0);\r\n", RobotGestureLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(175,281,'%d',0);\r\n", RobotGestureLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(189,281,'%d',0);\r\n", RobotGestureLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(204,281,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(219,281,'%d',0);\r\n", RobotGestureLevel[3]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(234,281,'%d',0);\r\n", RobotGestureLevel[4]); delay_ms(LCDRufusTime);
                                lcd_printf("CIR(255,285,3,0);\r\n"); delay_ms(LCDRufusTime);
                                RobotGestureLevelData = RobotGestureLevel[0] * 100.0f + RobotGestureLevel[1] * 10.0f +
                                                        RobotGestureLevel[2] * 1.0f + RobotGestureLevel[3] * 0.1f +
                                                        RobotGestureLevel[4] * 0.01f;
                                lcd_printf("BOXF(0,320,275,379,61);\r\n");
                                delay_ms(LCDRufusTime);
                                lcd_printf("DC32(31,341,'%d',0);\r\n", RobotGestureLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(45,341,'%d',0);\r\n", RobotGestureLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(59,341,'%d',0);\r\n", RobotGestureLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(74,341,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(89,341,'%d',0);\r\n", RobotGestureLevel[3]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(104,341,'%d',0);\r\n", RobotGestureLevel[4]); delay_ms(LCDRufusTime);
                                lcd_printf("CIR(125,345,3,0);\r\n"); delay_ms(LCDRufusTime);
                            }
                        }
                        CoordinateModeOld = CoordinateMode;
                    }
                    if (RobotSpeedMode == 0) {
                        if (CoordinatenumOld != Coordinatenum) {
                            JointnumOld = 0xff;
                            switch (Coordinatenum) {
                                case 1:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 2:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 3:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 4:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 5:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 6:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                            }
                            CoordinatenumOld = Coordinatenum;
                        }
                    } else if (RobotSpeedMode == 1) {
                        lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                        lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                        lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                        lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,'*',0);\r\n"); delay_ms(LCDRufusTime);

                    }

                    switch (Coordinatenum) {
                        case 1:


                            if (RobotChooseSta[0] > 0) {
                                if (RobotChooseSta[0] == 1) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[0] = RobotAxisLevelData * (1);
                                } else if (RobotChooseSta[0] == 2) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[0] = RobotAxisLevelData * (-1);
                                }
                                RobotDataStatusS[0] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 0;
                                SendDataToMain = 1;
                                SendCmdClass = 1;
                                RobotChooseSta[0] = 0;
                            }
                            break;
                        case 2:

                            if (RobotChooseSta[1] > 0) {
                                if (RobotChooseSta[1] == 1) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[1] = RobotAxisLevelData * (1);
                                } else if (RobotChooseSta[1] == 2) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[1] = RobotAxisLevelData * (-1);
                                }
                                RobotDataStatusS[1] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 1;
                                SendDataToMain = 1;
                                SendCmdClass = 2;
                                RobotChooseSta[1] = 0;
                            }
                            break;
                        case 3:

                            if (RobotChooseSta[2] > 0) {
                                if (RobotChooseSta[2] == 1) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[2] = RobotAxisLevelData * (1);
                                } else if (RobotChooseSta[2] == 2) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[2] = RobotAxisLevelData * (-1);
                                }
                                RobotDataStatusS[2] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 2;
                                SendDataToMain = 1;
                                SendCmdClass = 3;
                                RobotChooseSta[2] = 0;
                            }
                            break;
                        case 4:

                            if (RobotChooseSta[3] > 0) {
                                if (RobotChooseSta[3] == 1) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[3] = RobotGestureLevelData * (1);
                                } else if (RobotChooseSta[3] == 2) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[3] = RobotGestureLevelData * (-1);
                                }
                                RobotDataStatusS[3] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 3;
                                SendDataToMain = 1;
                                SendCmdClass = 4;
                                RobotChooseSta[3] = 0;
                            }
                            break;
                        case 5:

                            if (RobotChooseSta[4] > 0) {
                                if (RobotChooseSta[4] == 1) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[4] = RobotGestureLevelData * (1);
                                } else if (RobotChooseSta[4] == 2) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[4] = RobotGestureLevelData * (-1);
                                }
                                RobotDataStatusS[4] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 4;
                                SendDataToMain = 1;
                                SendCmdClass = 5;
                                RobotChooseSta[4] = 0;
                            }
                            break;
                        case 6:

                            if (RobotChooseSta[5] > 0) {
                                if (RobotChooseSta[5] == 1) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[5] = RobotGestureLevelData * (1);
                                } else if (RobotChooseSta[5] == 2) {
                                    lcd_printf("BOXF(15,340,29,379,61);DC32(16,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                    RobotSendData[5] = RobotGestureLevelData * (-1);
                                }
                                RobotDataStatusS[5] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 5;
                                SendDataToMain = 1;
                                SendCmdClass = 6;
                                RobotChooseSta[5] = 0;
                            }
                            break;
                    }

                }
                else if (ContralMode == 1) {
                    if(RobotJointIndex == 0){
                        if (SetMyFlg[2] == 0x01) {
                            SetMyFlg[2] = 0x00;
                            JointModeOld  = 0xff;
                            //RobotDataSendCmd = 1;
                        }
                    }
                    else if(RobotJointIndex>0){
                        SetMyFlg[2] = 0x01;
                        switch(RobotJointIndex){
                            case 1:
                                lcd_printf("DC32(161,281,'%d',0);\r\n",RobotJointLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(176,281,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(191,281,'%d',0);\r\n",RobotJointLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("BOXF(205,280,220,319,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(206,281,'%d',1);\r\n",RobotJointLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("CIR(226,288,3,0);\r\n"); delay_ms(LCDRufusTime);
                                break;
                            case 2:
                                lcd_printf("DC32(161,281,'%d',0);\r\n",RobotJointLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(176,281,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("BOXF(190,280,205,319,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(191,281,'%d',1);\r\n",RobotJointLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(206,281,'%d',0);\r\n",RobotJointLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("CIR(226,288,3,0);\r\n"); delay_ms(LCDRufusTime);
                                break;
                            case 3:
                                lcd_printf("BOXF(160,280,175,319,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(161,281,'%d',1);\r\n",RobotJointLevel[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(176,281,'.',0);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(191,281,'%d',0);\r\n",RobotJointLevel[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(206,281,'%d',0);\r\n",RobotJointLevel[2]); delay_ms(LCDRufusTime);
                                lcd_printf("CIR(226,288,3,0);\r\n"); delay_ms(LCDRufusTime);
                                break;
                        }
                    }
                    if(RobotJointDisPlay){
                        lcd_printf("BOXF(0,320,275,379,61);\r\n"); delay_ms(LCDRufusTime);
                        switch (Jointnum) {
                            case 1:
                                if(RobotSendData[6] >= 0){
                                    lcd_printf("DC32(15,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                else if(RobotSendData[6]<0){
                                    lcd_printf("DC32(15,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                lcd_printf("DC32(31,341,'%0.2f',0);\r\n", fabs(RobotSendData[6])); delay_ms(LCDRufusTime);
                                DataTemp = (uint32_t)fabs(RobotSendData[6]);
                                //usb_printf("DataTemp:%d,origin:%0.2f\r\n",DataTemp,fabs(RobotSendData[6]));
                                lcd_printf("CIR(%d,348,3,0);\r\n",31+15* GetDataBitNum(DataTemp)+53); delay_ms(LCDRufusTime);
                                break;
                            case 2:
                                if(RobotSendData[7] >= 0){
                                    lcd_printf("DC32(15,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                else if(RobotSendData[7]<0){
                                    lcd_printf("DC32(15,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                lcd_printf("DC32(31,341,'%0.2f',0);\r\n", fabs(RobotSendData[7])); delay_ms(LCDRufusTime);
                                DataTemp = (uint32_t)fabs(RobotSendData[7]);
                                //usb_printf("DataTemp:%d,origin:%0.2f\r\n",DataTemp,fabs(RobotSendData[7]));
                                lcd_printf("CIR(%d,348,3,0);\r\n",31+15* GetDataBitNum(DataTemp)+53); delay_ms(LCDRufusTime);
                                break;
                            case 3:
                                if(RobotSendData[8] >= 0){
                                    lcd_printf("DC32(15,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                else if(RobotSendData[8]<0){
                                    lcd_printf("DC32(15,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                lcd_printf("DC32(31,341,'%0.2f',0);\r\n", fabs(RobotSendData[8])); delay_ms(LCDRufusTime);
                                DataTemp = (uint32_t)fabs(RobotSendData[8]);
                                //usb_printf("DataTemp:%d,origin:%0.2f\r\n",DataTemp,fabs(RobotSendData[8]));
                                lcd_printf("CIR(%d,348,3,0);\r\n",31+15* GetDataBitNum(DataTemp)+53); delay_ms(LCDRufusTime);
                                break;
                            case 4:
                                if(RobotSendData[9] >= 0){
                                    lcd_printf("DC32(15,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                else if(RobotSendData[9]<0){
                                    lcd_printf("DC32(15,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                lcd_printf("DC32(31,341,'%0.2f',0);\r\n", fabs(RobotSendData[9])); delay_ms(LCDRufusTime);
                                DataTemp = (uint32_t)fabs(RobotSendData[9]);
                                //usb_printf("DataTemp:%d,origin:%0.2f\r\n",DataTemp,fabs(RobotSendData[9]));
                                lcd_printf("CIR(%d,348,3,0);\r\n",31+15* GetDataBitNum(DataTemp)+53); delay_ms(LCDRufusTime);
                                break;
                            case 5:
                                if(RobotSendData[10] >= 0){
                                    lcd_printf("DC32(15,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                else if(RobotSendData[10]<0){
                                    lcd_printf("DC32(15,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                lcd_printf("DC32(31,341,'%0.2f',0);\r\n", fabs(RobotSendData[10])); delay_ms(LCDRufusTime);
                                DataTemp = (uint32_t)fabs(RobotSendData[10]);
                                //usb_printf("DataTemp:%d,origin:%0.2f\r\n",DataTemp,fabs(RobotSendData[10]));
                                lcd_printf("CIR(%d,348,3,0);\r\n",31+15* GetDataBitNum(DataTemp)+53); delay_ms(LCDRufusTime);
                                break;
                            case 6:
                                if(RobotSendData[11] >= 0){
                                    lcd_printf("DC32(15,341,'+',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                else if(RobotSendData[11]<0){
                                    lcd_printf("DC32(15,341,'-',0);\r\n"); delay_ms(LCDRufusTime);
                                }
                                lcd_printf("DC32(31,341,'%0.2f',0);\r\n", fabs(RobotSendData[11])); delay_ms(LCDRufusTime);
                                DataTemp = (uint32_t)fabs(RobotSendData[11]);
                                //usb_printf("DataTemp:%d,origin:%0.2f\r\n",DataTemp,fabs(RobotSendData[11]));
                                lcd_printf("CIR(%d,348,3,0);\r\n",31+15* GetDataBitNum(DataTemp)+53); delay_ms(LCDRufusTime);
                                break;
                        }
                        RobotJointDisPlay = 0;
                    }
                    if(RobotJointSendCmd){
                        RobotJointSendCmd = 0;
                        RobotDataStatusFlag = 1;
                        switch (Jointnum) {
                            case 1:
                                RobotDataStatusS[6] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 6;
                                SendDataToMain = 1;
                                SendCmdClass = 7;
                                break;
                            case 2:
                                RobotDataStatusS[7] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 7;
                                SendDataToMain = 1;
                                SendCmdClass = 8;

                                break;
                            case 3:
                                RobotDataStatusS[8] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 8;
                                SendDataToMain = 1;
                                SendCmdClass = 9;

                                break;
                            case 4:
                                RobotDataStatusS[9] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 9;
                                SendDataToMain = 1;
                                SendCmdClass = 10;

                                break;
                            case 5:
                                RobotDataStatusS[10] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 10;
                                SendDataToMain = 1;
                                SendCmdClass = 11;

                                break;
                            case 6:
                                RobotDataStatusS[11] = 0;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 11;
                                SendDataToMain = 1;
                                SendCmdClass = 12;

                                break;
                        }
                    }

                    if (JointModeOld != JointMode) {
                        lcd_printf("BOXF(0,280,159,319,61);DC32(1,281,'[J1-J6] X ',0);\r\n"); delay_ms(LCDRufusTime);
                        CoordinateModeOld = 0xff;
                        if (JointMode == 1) {
                            lcd_printf("BOXF(0,0,140,39,61);DC32(1,1,'J1:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,80,140,119,61);DC32(1,81,'J2:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,160,140,199,61);DC32(1,161,'J3:',0);\r\n"); delay_ms(LCDRufusTime);
                        } else if (JointMode == 2) {
                            lcd_printf("BOXF(0,0,140,39,61);DC32(1,1,'J4:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,80,140,119,61);DC32(1,81,'J5:',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,160,140,199,61);DC32(1,161,'J6:',0);\r\n"); delay_ms(LCDRufusTime);
                        }

                        if (RobotJointIndex  == 0 ) {
                            lcd_printf("BOXF(160,280,319,319,61);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("DC32(161,281,'%d',0);\r\n",RobotJointLevel[0]); delay_ms(LCDRufusTime);
                            lcd_printf("DC32(176,281,'.',0);\r\n"); delay_ms(LCDRufusTime);
                            lcd_printf("DC32(191,281,'%d',0);\r\n",RobotJointLevel[1]); delay_ms(LCDRufusTime);
                            lcd_printf("DC32(206,281,'%d',0);\r\n",RobotJointLevel[2]); delay_ms(LCDRufusTime);
                            lcd_printf("CIR(226,288,3,0);\r\n");
                            RobotJointLevelData = RobotJointLevel[0]*1.0f+RobotJointLevel[1]*0.1f+RobotJointLevel[2]*0.01f;
                        }
                        JointModeOld = JointMode;
                    }
                    if (RobotSpeedMode == 0) {
                        if (JointnumOld != Jointnum) {
                            CoordinatenumOld = 0xff;
                            switch (Jointnum) {
                                case 1:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 2:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 3:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 4:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 5:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                                case 6:
                                    lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,'*',0);\r\n"); delay_ms(LCDRufusTime);
                                    lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,' ',0);\r\n"); delay_ms(LCDRufusTime);
                                    break;
                            }
                            JointnumOld = Jointnum;
                        }
                    } else if (RobotSpeedMode == 1) {

                        lcd_printf("BOXF(141,0,159,39,61);DC32(142,1,' ',0);\r\n"); delay_ms(LCDRufusTime);
                        lcd_printf("BOXF(141,80,159,119,61);DC32(142,81,' ',0);\r\n"); delay_ms(LCDRufusTime);
                        lcd_printf("BOXF(141,160,159,199,61);DC32(142,161,' ',0);\r\n"); delay_ms(LCDRufusTime);
                        lcd_printf("BOXF(180,390,199,439,61);DC32(181,391,'*',0);\r\n"); delay_ms(LCDRufusTime);
                    }
                }

                // lcd_printf("BOXF(160,0,319,39,61);DC32(161,1,'[%S]',0);\r\n",RobotStatus[ReadCharge]);
                if(robot_working0ld != robot_working){
                    lcd_printf("BOXF(160,0,319,39,61);DC32(161,1,'[%S]',0);\r\n",RobotStatus[robot_working]); delay_ms(LCDRufusTime);
                    robot_working0ld = robot_working;

                    if(robot_working == 4){
                        SaveRobotSta = 1;
                    }
                    else {
                        SaveRobotSta = 2;
                    }
                }

                if(DataChangeFlag){
                    DataChangeFlag = 0;
                    if(ContralMode == 0){
                        if(CoordinateMode == 1){
                            lcd_printf("BOXF(0,40,319,79,61);DC32(40,41,'%0.4f',0);\r\n",RobotInfo[0]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,121,319,160,61);DC32(40,121,'%0.4f',0);\r\n",RobotInfo[1]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,200,319,239,61);DC32(40,201,'%0.4f',0);\r\n",RobotInfo[2]);
                            delay_ms(LCDRufusTime);
                        }
                        else if(CoordinateMode == 2){
                            lcd_printf("BOXF(0,40,319,79,61);DC32(40,41,'%0.4f',0);\r\n",RobotInfo[3]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,121,319,160,61);DC32(40,121,'%0.4f',0);\r\n",RobotInfo[4]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,200,319,239,61);DC32(40,201,'%0.4f',0);\r\n",RobotInfo[5]);
                            delay_ms(LCDRufusTime);
                        }
                    }
                    if(ContralMode == 1){
                        if(JointMode == 1){
                            lcd_printf("BOXF(0,40,319,79,61);DC32(40,41,'%0.4f',0);\r\n",RobotInfo[6]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,121,319,160,61);DC32(40,121,'%0.4f',0);\r\n",RobotInfo[7]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,200,319,239,61);DC32(40,201,'%0.4f',0);\r\n",RobotInfo[8]);
                            delay_ms(LCDRufusTime);
                        }
                        else if(JointMode == 2){
                            lcd_printf("BOXF(0,40,319,79,61);DC32(40,41,'%0.4f',0);\r\n",RobotInfo[9]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,121,319,160,61);DC32(40,121,'%0.4f',0);\r\n",RobotInfo[10]);
                            delay_ms(LCDRufusTime);
                            lcd_printf("BOXF(0,200,319,239,61);DC32(40,201,'%0.4f',0);\r\n",RobotInfo[11]);
                            delay_ms(LCDRufusTime);
                        }
                    }
                }
                if(RobotSpeedChange == 1){
                    if(RobotSpeedIndex == 0){
                        lcd_printf("BOXF(109,390,155,439,61);\r\n"); delay_ms(LCDRufusTime);
                        RobotSpeedChange = 0;

                    }
                    else if(RobotSpeedIndex>0){
                        switch (RobotSpeedIndex) {
                            case 1:

                                lcd_printf("DC32(110,391,'%d',0);\r\n",RobotSpeedData[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(125,391,'%d',0);\r\n",RobotSpeedData[1]); delay_ms(LCDRufusTime);
                                lcd_printf("BOXF(139,390,154,439,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(140,391,'%d',1);\r\n",RobotSpeedData[2]); delay_ms(LCDRufusTime);
                                //lcd_printf("CIR(160,390,3,0);\r\n");
                                break;
                            case 2:

                                lcd_printf("DC32(110,391,'%d',0);\r\n",RobotSpeedData[0]); delay_ms(LCDRufusTime);
                                lcd_printf("BOXF(124,390,139,439,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(125,391,'%d',1);\r\n",RobotSpeedData[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(140,391,'%d',0);\r\n",RobotSpeedData[2]); delay_ms(LCDRufusTime);
                                //lcd_printf("CIR(160,390,3,0);\r\n");
                                break;
                            case 3:

                                lcd_printf("BOXF(109,390,124,439,61);\r\n"); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(110,391,'%d',1);\r\n",RobotSpeedData[0]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(125,391,'%d',0);\r\n",RobotSpeedData[1]); delay_ms(LCDRufusTime);
                                lcd_printf("DC32(140,391,'%d',0);\r\n",RobotSpeedData[2]); delay_ms(LCDRufusTime);
                                //lcd_printf("CIR(160,390,3,0);\r\n");
                                break;
                        }
                    }
                }
                else if(RobotSpeedChange == 0){

                    lcd_printf("DC32(110,391,'%d',0);\r\n",RobotSpeedData[0]); delay_ms(LCDRufusTime);
                    lcd_printf("DC32(125,391,'%d',0);\r\n",RobotSpeedData[1]); delay_ms(LCDRufusTime);
                    lcd_printf("DC32(140,391,'%d',0);\r\n",RobotSpeedData[2]); delay_ms(LCDRufusTime);
                    //lcd_printf("CIR(160,390,3,0);\r\n");
                    RobotSpeedValue = RobotSpeedData[0]*100+RobotSpeedData[1]*10+RobotSpeedData[2];


                }

                if(global_percentold != global_percentDis){

                    lcd_printf("BOXF(179,440,319,479,61);DC32(180,441,'Bat:%d%%',0);\r\n",global_percentDis); delay_ms(LCDRufusTime);
                    global_percentold = global_percentDis;
                }
                if(RobotDataStatusFlag){
                    RobotDataStatusFlag = 0;
                    SetSendRobotStatus(RobotDataStatusS[RobotDataSendIndex]);
                }

            }
        }
        else if(SystemLock == 0 && LCDLock == 0){
            if(LCDLock == 0){
                lcd_printf("BL(255);\r\n");
            }
        }

        else if(SystemLock == 0 && LCDLock == 1){
            if(LCDLock == 0){
                lcd_printf("BL(255);\r\n");
            }
        }
        vTaskDelay(1);
    }
}
void System(void *arg){
    uint16_t  systemcnt = 0;
    uint8_t syslock = 0;
    while(1){
        if(SystemLock == 0){
            systemcnt = 0;
            if(HAL_GPIO_ReadPin(KeyOn_GPIO_Port,KeyOn_Pin) == 1){
                HAL_GPIO_WritePin(PowerOn_GPIO_Port,PowerOn_Pin,GPIO_PIN_SET);
                syslock = 1;

            }
            else if(HAL_GPIO_ReadPin(KeyOn_GPIO_Port,KeyOn_Pin) == 0) {
                if (syslock == 1) {
                    syslock = 0;
                    SystemLock = 1;
                    LCDLock = 1;
                    XbeeWork = 1;
                }
            }
        }
        else if(SystemLock) {
            if (HAL_GPIO_ReadPin(KeyOn_GPIO_Port, KeyOn_Pin) == 1) {
                if (systemcnt++ >= 500) {
                    systemcnt = 0;
                    syslock = 1;
                    LCDLock = 0;
                    XbeeWork = 0;
                }

            } else if (HAL_GPIO_ReadPin(KeyOn_GPIO_Port, KeyOn_Pin) == 0) {
                if (syslock == 1) {
                    syslock = 0;
                    SystemLock = 0;
                    HAL_GPIO_WritePin(PowerOn_GPIO_Port, PowerOn_Pin, GPIO_PIN_RESET);
                }
            }
        }

        vTaskDelay(1);
    }
}
void keyscan(void *arg){
    static uint16_t PowerCnt = 0;
    static uint8_t  KeyLock[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
    static uint16_t KeyCnt = 0;
    static uint8_t  UnlockMode = 0;
    static uint8_t  modechangflag = 0;
    while(1){

        if(SystemLock) {
            if(++KeyCnt >= KeyScanTime){
                KeyCnt = 0;
                if(ReadLeveladd == KeyPress && !KeyLock[0]){
                    KeyLock[0] = 1;
                    ContralMode ^= 1;

                    RobotDataStatusFlag = 1;
                    ClearRobotSendStatus();
                    if(ContralMode == 0){
                        Jointnum = 1;
                        SetMyFlg[2] = 0;
                        RobotJointIndex = 0;
                        JointModeOld = 0xff;

                    }
                    if(CoordinateMode == 1){
                        Coordinatenum = 1;
                        RobotAxisIndex = 0;
                        RobotGestureIndex = 0;
                        SetMyFlg[1] = 0;
                        SetMyFlg[0] = 0;
                        CoordinateModeOld = 0xff;

                    }
                    RobotSpeedMode = 0;
                }
                else if(ReadLeveladd == KeyOpen ){
                    KeyLock[0] = 0;
                }
                if(ReadLevelsub == KeyPress && !KeyLock[1]){
                    KeyLock[1] = 1;
                    ClearRobotSendStatus();
                    RobotDataStatusFlag = 1;
                    if(ContralMode == 0){
                        JointMode = 1;
                        if(++CoordinateMode>=3){
                            CoordinateMode = 1;
                            UnlockMode = 1;
                        }
                        CoordinateModeOld = 0xff;
                        JointModeOld = 0xff;
                        RobotGestureIndex = 0;
                        RobotAxisIndex = 0;
                        RobotSpeedMode = 0;

                        if(CoordinateMode == 1){
                            SetMyFlg[1] = 0;
                        }
                        else if(CoordinateMode == 2){
                            SetMyFlg[0] = 0;
                        }
                    }
                    else if(ContralMode == 1){
                        CoordinateMode = 1;
                        RobotSpeedMode = 0;
                        CoordinateModeOld = 0xff;
                        if(++Jointnum>=7){
                            Jointnum = 1;
                        }
                        RobotJointDisPlay = 1;
                        if(RobotJointIndex> 0){
                            RobotJointIndex = 0;
                            SetMyFlg[2] = 1;

                        }
                    }
                }
                else if(ReadLevelsub == KeyOpen ){
                    KeyLock[1] = 0;
                }
                if(ReadXsub == KeyPress && !KeyLock[2]){
                    KeyLock[2] = 1;
                    if(ContralMode == 0){
                        ////RobotDataSendCmd = 1;
                        if(CoordinateMode == 1){
                            Coordinatenum = 1;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotAxisIndex> 0){
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 1;
                            }
                            else if(RobotAxisIndex == 0){
                                SetMyFlg[0] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[0] = 2;
                        }
                        if(CoordinateMode == 2){
                            Coordinatenum = 4;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotGestureIndex> 0){
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 1;
                            }
                            else if(RobotGestureIndex == 0){
                                SetMyFlg[1] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[3] = 2;
                        }
                    }
                }
                else if(ReadXsub == KeyOpen){
                    KeyLock[2] = 0;
                }
                if(ReadXadd == KeyPress && !KeyLock[3]){
                    KeyLock[3] = 1;
                    if(ContralMode == 0){
                        // //RobotDataSendCmd = 1;
                        if(CoordinateMode == 1){
                            Coordinatenum = 1;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotAxisIndex> 0){
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 1;

                            }
                            else if(RobotAxisIndex == 0){
                                SetMyFlg[0] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[0] = 1;
                        }
                        if(CoordinateMode == 2){
                            Coordinatenum = 4;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotGestureIndex> 0){
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 1;
                            }
                            else if(RobotGestureIndex == 0){
                                SetMyFlg[1] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[3] = 1;
                        }
                    }
                }
                else if(ReadXadd == KeyOpen){
                    KeyLock[3] = 0;
                }
                if(ReadYsub == KeyPress && !KeyLock[4]){
                    KeyLock[4] = 1;
                    if(ContralMode == 0){
                        // //RobotDataSendCmd = 1;
                        if(CoordinateMode == 1){
                            Coordinatenum = 2;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotAxisIndex> 0){
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 1;
                            }
                            else if(RobotAxisIndex == 0){
                                SetMyFlg[0] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[1] = 2;
                        }
                        if(CoordinateMode == 2){
                            Coordinatenum = 5;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotGestureIndex> 0){
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 1;
                            }
                            else if(RobotGestureIndex == 0){
                                SetMyFlg[1] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[4] = 2;
                        }
                    }
                }
                else if(ReadYsub == KeyOpen){
                    KeyLock[4] = 0;
                }
                if(ReadYadd == KeyPress && !KeyLock[5]){
                    KeyLock[5] = 1;
                    if(ContralMode == 0){
                        // //RobotDataSendCmd = 1;
                        if(CoordinateMode == 1){
                            Coordinatenum = 2;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotAxisIndex> 0){
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 1;
                            }
                            else if(RobotAxisIndex == 0){
                                SetMyFlg[0] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[1] = 1;
                        }
                        if(CoordinateMode == 2){
                            Coordinatenum = 5;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotGestureIndex> 0){
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 1;
                            }
                            else if(RobotGestureIndex == 0){
                                SetMyFlg[1] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[4] = 1;
                        }
                    }
                }
                else if(ReadYadd == KeyOpen){
                    KeyLock[5] = 0;
                }
                if(ReadZsub == KeyPress && !KeyLock[6]){
                    KeyLock[6] = 1;
                    if(ContralMode == 0){
                        //  //RobotDataSendCmd = 1;
                        if(CoordinateMode == 1){
                            Coordinatenum = 3;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotAxisIndex> 0){
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 1;
                            }
                            else if(RobotAxisIndex == 0){
                                SetMyFlg[0] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[2] = 2;
                        }
                        if(CoordinateMode == 2){
                            Coordinatenum = 6;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotGestureIndex> 0){
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 1;
                            }
                            else if(RobotGestureIndex == 0){
                                SetMyFlg[1] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[5] = 2;
                        }
                    }
                }
                else if(ReadZsub == KeyOpen){
                    KeyLock[6] = 0;
                }
                if(ReadZadd == KeyPress && !KeyLock[7]){
                    KeyLock[7] = 1;
                    if(ContralMode == 0){
                        // //RobotDataSendCmd = 1;
                        if(CoordinateMode == 1){
                            Coordinatenum = 3;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotAxisIndex> 0){
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 1;
                            }
                            else if(RobotAxisIndex == 0){
                                SetMyFlg[0] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[2] = 1;
                        }
                        if(CoordinateMode == 2){
                            Coordinatenum = 6;

                            if(RobotSpeedMode == 1){
                                RobotSpeedMode = 0;
                                RobotSpeedIndex = 0;

                            }
                            if(RobotGestureIndex> 0){
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 1;
                            }
                            else if(RobotGestureIndex == 0){
                                SetMyFlg[1] = 0;
                                //RobotDataSendCmd = 1;
                            }
                            RobotChooseSta[5] = 1;
                        }
                    }
                }
                else if(ReadZadd == KeyOpen){
                    KeyLock[7] = 0;
                }
                if(ReadPauseKey == KeyPress && !KeyLock[8]){
                    KeyLock[8] = 1;
                    //SystemWorkLock = 1;
#ifdef ModeContral
                    SendFlah = 1;
#endif
                }
                else if(ReadPauseKey == KeyOpen){
                    KeyLock[8] = 0;
                }
                if(ReadModeKey == KeyPress && !KeyLock[9]){
                    KeyLock[9] = 1;
                    RobotSpeedMode ^= 1;
                    JointnumOld = 0xff;
                    CoordinateModeOld = 0xff;
                    CoordinatenumOld = 0xff;
                }
                else if(ReadModeKey == KeyOpen){
                    KeyLock[9] = 0;
                }
                if(ReadJointadd == KeyPress &&!KeyLock[10]){
                    KeyLock[10] = 1;
                    if((ReadLeveladd * ReadLevelsub) == 1) {
                        if (RobotSpeedMode == 1) {

                            RobotSpeedIndex++;
                            if (RobotSpeedIndex >= 4) {
                                RobotSpeedIndex = 0;
                            }
                        } else if (RobotSpeedMode == 0) {
                            if (ContralMode == 0) {
                                if (CoordinateMode == 1) {
                                    RobotJointIndex = 0;
                                    SetMyFlg[2] = 0;
                                    RobotGestureIndex = 0;
                                    SetMyFlg[1] = 0;
                                    RobotAxisIndex++;
                                    if (RobotAxisIndex >= 4) {
                                        RobotAxisIndex = 0;
                                    }
                                } else if (CoordinateMode == 2) {
                                    RobotJointIndex = 0;
                                    SetMyFlg[2] = 0;
                                    RobotAxisIndex = 0;
                                    SetMyFlg[0] = 0;
                                    RobotGestureIndex++;
                                    if (RobotGestureIndex >= 6) {
                                        RobotGestureIndex = 0;
                                    }
                                }
                            } else if (ContralMode == 1) {
                                RobotJointIndex++;
                                RobotAxisIndex = 0;
                                SetMyFlg[0] = 0;
                                RobotGestureIndex = 0;
                                SetMyFlg[1] = 0;
                                if (RobotJointIndex >= 4) {
                                    RobotJointIndex = 0;
                                }
                            }
                        }
                    }
                }
                else if(ReadJointadd == KeyOpen){
                    KeyLock[10] = 0;
                }

                if(ReadJointsub == KeyPress &&!KeyLock[11]){
                    KeyLock[11] = 1;
                    if(RobotSpeedMode == 1){
                        if(++RobotSpeedData[3-RobotSpeedIndex]>=10){
                            RobotSpeedData[3-RobotSpeedIndex] = 0;
                        }
                    }
                    else if(RobotSpeedMode == 0){
                        if(ContralMode == 0){
                            if(CoordinateMode == 1){
                                if(RobotAxisIndex>0&&RobotAxisIndex<3){
                                    if(++RobotAxisLevel[3-RobotAxisIndex]>=10){
                                        RobotAxisLevel[3-RobotAxisIndex] = 0;
                                    }
                                }
                                if(RobotAxisIndex == 3){
                                    if(++RobotAxisLevel[3-RobotAxisIndex]>=10){
                                        RobotAxisLevel[3-RobotAxisIndex] = 0;
                                    }

                                }
                            }
                            else if(CoordinateMode == 2){
                                if(++RobotGestureLevel[5-RobotGestureIndex]>=10){
                                    RobotGestureLevel[5-RobotGestureIndex] = 0;
                                }
                            }
                        }
                        else if(ContralMode == 1){
                            if(++RobotJointLevel[3-RobotJointIndex]>=10){
                                RobotJointLevel[3-RobotJointIndex] = 0;
                            }
                        }
                    }
                }
                else if(ReadJointsub == KeyOpen){
                    KeyLock[11] = 0;
                }

            }


            if(RobotSpeedMode){
                if(RobotSpeedIndex>0){
                    RobotSpeedChange = 1;
                }
            }else if(RobotSpeedMode == 0){
                RobotSpeedChange = 0;
                RobotSpeedIndex = 0;
            }



            if(ContralMode){
                if(Jointnum>=1 && Jointnum<=3){
                    JointMode = 1;
                }
                else if(Jointnum>=4 && Jointnum<=6){
                    JointMode = 2;
                }
            }



            if(UnlockMode){
                UnlockMode = 0;
                if(CoordinateMode == 1){
                    Coordinatenum = 1;
                    CoordinatenumOld = 0xff;

                }
                else if(CoordinateMode == 2){
                    Coordinatenum = 4;
                    CoordinatenumOld = 0xff;

                }
            }
            if(SystemWorkLock){
                SystemWorkLock = 0;
#ifdef ModeToMore
                modechangflag^=1;
                if(modechangflag == 1){
                    SendDataToMain = 1;
                    SendCmdClass = 13;
                }
                else if(modechangflag == 0){
                    SendDataToMain = 1;
                    SendCmdClass = 14;
                }
#else
                if(SaveRobotSta == 1){
                    SendDataToMain = 1;
                    SendCmdClass = 14;
                    //恢复
                }else if(SaveRobotSta == 2){
                    SendDataToMain = 1;
                    SendCmdClass = 13;
                }
                else if(SaveRobotSta == 0){
                    SendDataToMain = 0;
                    SendCmdClass = 0;
                }
#endif
            }
        }
        vTaskDelay(1);
    }
}
void ledentry(void *arg){
    static uint16_t ledcnt = LEDScanTime,Chargecnt = LEDChargrTime;
    uint8_t changeflg = 0;
    uint8_t  num = 0,numold = 0,LedIndex = 0;
    uint8_t global_percentoldled = 0;
    while(1){
        if(SystemLock == 1){
            if(ReadCharge) {
                Chargecnt = 0;
                numold = 0xff;
                if(global_percentoldled != global_percent){
                    if(global_percent>global_percentoldled){
                        global_percent = global_percentoldled;
                    }
                    else {
                        global_percent = global_percent;
                    }
                    global_percentoldled = global_percent;
                }
                if (++ledcnt >= LEDScanTime) {
                    ledcnt = 0;
                    changeflg ^= 1;

                    if (global_percent > 98 && global_percent <= 100) {
                        WS281x_Show_SetNum(0, 50, 0, 53);
                    }
                    if (global_percent > 96 && global_percent <= 98) {
                        WS281x_Show_SetNum(0, 50, 0, 52);
                    }
                    if (global_percent > 94 && global_percent <= 96) {
                        WS281x_Show_SetNum(0, 50, 0, 51);
                    }
                    if (global_percent > 92 && global_percent <= 94) {
                        WS281x_Show_SetNum(0, 50, 0, 50);
                    }
                    if (global_percent > 90 && global_percent <= 92) {
                        WS281x_Show_SetNum(0, 50, 0, 49);
                    }
                    if (global_percent > 88 && global_percent <= 90) {
                        WS281x_Show_SetNum(0, 50, 0, 48);
                    }
                    if (global_percent > 86 && global_percent <= 88) {
                        WS281x_Show_SetNum(0, 50, 0, 47);
                    }
                    if (global_percent > 84 && global_percent <= 86) {
                        WS281x_Show_SetNum(0, 50, 0, 46);
                    }
                    if (global_percent > 82 && global_percent <= 84) {
                        WS281x_Show_SetNum(0, 50, 0, 45);
                    }
                    if (global_percent > 80 && global_percent <= 82) {
                        WS281x_Show_SetNum(0, 50, 0, 44);
                    }
                    if (global_percent > 78 && global_percent <= 80) {
                        WS281x_Show_SetNum(0, 50, 0, 43);
                    }
                    if (global_percent > 76 && global_percent <= 78) {
                        WS281x_Show_SetNum(0, 50, 0, 42);
                    }
                    if (global_percent > 74 && global_percent <= 76) {
                        WS281x_Show_SetNum(0, 50, 0, 41);
                    }
                    if (global_percent > 72 && global_percent <= 74) {
                        WS281x_Show_SetNum(0, 50, 0, 40);
                    }
                    if (global_percent > 70 && global_percent <= 72) {
                        WS281x_Show_SetNum(0, 50, 0, 39);
                    }
                    if (global_percent > 68 && global_percent <= 70) {
                        WS281x_Show_SetNum(0, 50, 0, 38);
                    }
                    if (global_percent > 66 && global_percent <= 68) {
                        WS281x_Show_SetNum(0, 50, 0, 37);
                    }
                    if (global_percent > 64 && global_percent <= 66) {
                        WS281x_Show_SetNum(0, 50, 0, 36);
                    }
                    if (global_percent > 62 && global_percent <= 64) {
                        WS281x_Show_SetNum(0, 50, 0, 35);
                    }
                    if (global_percent > 60 && global_percent <= 62) {
                        WS281x_Show_SetNum(0, 50, 0, 34);
                    }
                    if (global_percent > 58 && global_percent <= 60) {
                        WS281x_Show_SetNum(0, 50, 0, 33);
                    }
                    if (global_percent > 56 && global_percent <= 58) {
                        WS281x_Show_SetNum(0, 50, 0, 32);
                    }
                    if (global_percent > 54 && global_percent <= 56) {
                        WS281x_Show_SetNum(0, 50, 0, 31);
                    }
                    if (global_percent > 52 && global_percent <= 54) {
                        WS281x_Show_SetNum(0, 50, 0, 30);
                    }
                    if (global_percent > 50 && global_percent <= 52) {
                        WS281x_Show_SetNum(0, 50, 0, 29);
                    }
                    if (global_percent > 48 && global_percent <= 50) {
                        WS281x_Show_SetNum(0, 50, 0, 28);
                    }
                    if (global_percent > 46 && global_percent <= 48) {
                        if (global_percent > 47 && global_percent <= 48) {
                            WS281x_Show_SetNum(0, 50, 0, 27);
                        }
                        if (global_percent > 46 && global_percent <= 47) {
                            WS281x_Show_SetNum(51, 30, 1, 27);
                        }

                    }


                    if (global_percent > 45 && global_percent <= 46) {
                        WS281x_Show_SetNum(51, 30, 1, 26);
                    }
                    if (global_percent > 44 && global_percent <= 45) {
                        WS281x_Show_SetNum(51, 30, 1, 25);
                    }
                    if (global_percent > 43 && global_percent <= 44) {
                        WS281x_Show_SetNum(51, 30, 1, 24);
                    }
                    if (global_percent > 42 && global_percent <= 43) {
                        WS281x_Show_SetNum(51, 30, 1, 23);
                    }
                    if (global_percent > 41 && global_percent <= 42) {
                        WS281x_Show_SetNum(51, 30, 1, 22);
                    }
                    if (global_percent > 40 && global_percent <= 41) {
                        WS281x_Show_SetNum(51, 30, 1, 21);
                    }
                    if (global_percent > 39 && global_percent <= 40) {
                        WS281x_Show_SetNum(51, 30, 1, 20);
                    }
                    if (global_percent > 38 && global_percent <= 39) {
                        WS281x_Show_SetNum(51, 30, 1, 19);
                    }
                    if (global_percent > 37 && global_percent <= 38) {
                        WS281x_Show_SetNum(51, 30, 1, 18);
                    }
                    if (global_percent > 36 && global_percent <= 37) {
                        WS281x_Show_SetNum(51, 30, 1, 17);
                    }
                    if (global_percent > 35 && global_percent <= 36) {
                        WS281x_Show_SetNum(51, 30, 1, 16);
                    }
                    if (global_percent > 34 && global_percent <= 35) {
                        WS281x_Show_SetNum(51, 30, 1, 15);
                    }
                    if (global_percent > 33 && global_percent <= 34) {
                        WS281x_Show_SetNum(51, 30, 1, 14);
                    }
                    if (global_percent > 32 && global_percent <= 33) {
                        WS281x_Show_SetNum(51, 30, 1, 13);
                    }
                    if (global_percent > 31 && global_percent <= 32) {
                        WS281x_Show_SetNum(51, 0, 1, 13);
                    }
                    if (global_percent > 30 && global_percent <= 31) {
                        WS281x_Show_SetNum(51, 0, 1, 12);
                    }
                    if (global_percent > 29 && global_percent <= 30) {
                        WS281x_Show_SetNum(51, 0, 1, 11);
                    }
                    if (global_percent > 28 && global_percent <= 29) {
                        WS281x_Show_SetNum(51, 0, 1, 10);
                    }
                    if (global_percent > 27 && global_percent <= 28) {
                        WS281x_Show_SetNum(51, 0, 1, 9);
                    }
                    if (global_percent > 26 && global_percent <= 27) {
                        WS281x_Show_SetNum(51, 0, 1, 8);
                    }
                    if (global_percent > 25 && global_percent <= 26) {
                        WS281x_Show_SetNum(51, 0, 1, 7);
                    }
                    if (global_percent > 24 && global_percent <= 25) {
                        WS281x_Show_SetNum(51, 0, 1, 6);
                    }
                    if (global_percent > 23 && global_percent <= 24) {
                        WS281x_Show_SetNum(51, 0, 1, 5);
                    }
                    if (global_percent > 22 && global_percent <= 23) {
                        WS281x_Show_SetNum(51, 0, 1, 4);
                    }
                    if (global_percent > 22 && global_percent <= 23) {
                        WS281x_Show_SetNum(51, 0, 1, 3);
                    }
                    if (global_percent > 0 && global_percent <= 22) {
                        if (changeflg == 1) {
                            WS281x_Show_SetNum(51, 0, 1, 3);
                        } else if (changeflg == 0) {
                            WS281x_Show_SetNum(0, 0, 0, 3);
                        }
                    }

                }
            }
            if(ReadCharge == 0){
                ledcnt = 0;
                changeflg = 0;
                if(global_percentoldled != global_percent){
                    if(global_percent<global_percentoldled){
                        global_percent = global_percentoldled;
                    }
                    else {
                        global_percent = global_percent;
                    }
                    global_percentoldled = global_percent;
                }
                if(++Chargecnt>=LEDChargrTime){
                    Chargecnt = 0;
                    if (global_percent > 98 && global_percent <= 100) {
                        num = 53;
                    }
                    if (global_percent > 96 && global_percent <= 98) {
                        num = 52;
                    }
                    if (global_percent > 94 && global_percent <= 96) {
                        num = 51;
                    }
                    if (global_percent > 92 && global_percent <= 94) {
                        num = 50;
                    }
                    if (global_percent > 90 && global_percent <= 92) {
                        num = 49;
                    }
                    if (global_percent > 88 && global_percent <= 90) {
                        num = 48;
                    }
                    if (global_percent > 86 && global_percent <= 88) {
                        num = 47;
                    }
                    if (global_percent > 84 && global_percent <= 86) {
                        num = 46;
                    }
                    if (global_percent > 82 && global_percent <= 84) {
                        num = 45;
                    }
                    if (global_percent > 80 && global_percent <= 82) {
                        num = 44;
                    }
                    if (global_percent > 78 && global_percent <= 80) {
                        num = 43;
                    }
                    if (global_percent > 76 && global_percent <= 78) {
                        num = 42;
                    }
                    if (global_percent > 74 && global_percent <= 76) {
                        num = 41;
                    }
                    if (global_percent > 72 && global_percent <= 74) {
                        num = 40;
                    }
                    if (global_percent > 70 && global_percent <= 72) {
                        num = 39;
                    }
                    if (global_percent > 68 && global_percent <= 70) {
                        num = 38;
                    }
                    if (global_percent > 66 && global_percent <= 68) {
                        num = 37;
                    }
                    if (global_percent > 64 && global_percent <= 66) {
                        num = 36;
                    }
                    if (global_percent > 62 && global_percent <= 64) {
                        num = 35;
                    }
                    if (global_percent > 60 && global_percent <= 62) {
                        num = 34;
                    }
                    if (global_percent > 58 && global_percent <= 60) {
                        num = 33;
                    }
                    if (global_percent > 56 && global_percent <= 58) {
                        num = 32;
                    }
                    if (global_percent > 54 && global_percent <= 56) {
                        num = 31;
                    }
                    if (global_percent > 52 && global_percent <= 54) {
                        num = 30;
                    }
                    if (global_percent > 50 && global_percent <= 52) {
                        num = 29;
                    }
                    if (global_percent > 48 && global_percent <= 50) {
                        num = 28;
                    }
                    if (global_percent > 46 && global_percent <= 48) {
                        num = 27;
                    }


                    if (global_percent > 45 && global_percent <= 46) {
                        num = 26;
                    }
                    if (global_percent > 44 && global_percent <= 45) {
                        num = 25;
                    }
                    if (global_percent > 43 && global_percent <= 44) {
                        num = 24;
                    }
                    if (global_percent > 42 && global_percent <= 43) {
                        num = 23;
                    }
                    if (global_percent > 41 && global_percent <= 42) {
                        num = 22;
                    }
                    if (global_percent > 40 && global_percent <= 41) {
                        num = 21;
                    }
                    if (global_percent > 39 && global_percent <= 40) {
                        num = 20;
                    }
                    if (global_percent > 38 && global_percent <= 39) {
                        num = 19;
                    }
                    if (global_percent > 37 && global_percent <= 38) {
                        num = 18;
                    }
                    if (global_percent > 36 && global_percent <= 37) {
                        num = 17;
                    }
                    if (global_percent > 35 && global_percent <= 36) {
                        num = 16;
                    }
                    if (global_percent > 34 && global_percent <= 35) {
                        num = 15;
                    }
                    if (global_percent > 33 && global_percent <= 34) {
                        num = 14;
                    }
                    if (global_percent > 32 && global_percent <= 33) {
                        num = 13;
                    }
                    if (global_percent > 31 && global_percent <= 32) {
                        num = 13;
                    }
                    if (global_percent > 30 && global_percent <= 31) {
                        num = 12;
                    }
                    if (global_percent > 29 && global_percent <= 30) {
                        num = 11;
                    }
                    if (global_percent > 28 && global_percent <= 29) {
                        num = 10;
                    }
                    if (global_percent > 27 && global_percent <= 28) {
                        num = 9;
                    }
                    if (global_percent > 26 && global_percent <= 27) {
                        num = 8;
                    }
                    if (global_percent > 25 && global_percent <= 26) {
                        num = 7;
                    }
                    if (global_percent > 24 && global_percent <= 25) {
                        num = 6;
                    }
                    if (global_percent > 23 && global_percent <= 24) {
                        num = 5;
                    }
                    if (global_percent > 22 && global_percent <= 23) {
                        num = 4;
                    }
                    if (global_percent > 22 && global_percent <= 23) {
                        num = 3;
                    }
                    if (global_percent > 20&& global_percent <= 22) {
                        num = 2;
                    }
                    if (global_percent > 0&& global_percent <= 20) {
                        num = 1;
                    }

                    if(numold != num){
                        LedIndex = num;
                        numold = num;
                    }
                    WS2812x_ChargePrecoss(LedIndex++);
                    if(LedIndex>Pixel_NUM){
                        LedIndex = num;
                    }
                }
            }
        }
        else if(SystemLock == 0){
            ledcnt = 0;
            WS2812C_CloseALL();
        }

        vTaskDelay(1);
    }
}
void xbeesend(void *arg){
    static uint16_t  SendCnt = 0,tickcnt = 0;
    static float temp_data = 0.0f;
    while(1){
        if(SystemLock){
            if(++SendCnt>=XbeeSendTime) {
                SendCnt = 0;
                if (SendDataToMain == 0) {
                    if (tickcnt++ >= XbeeSendTickTime) {
                        tickcnt = 0;
                        ChenckRobotInfo();
                    }
                } else if (SendDataToMain == 1) {
                    tickcnt = 0;
                    SendDataToMain = 0;
                    if (SendCmdClass) {
                        switch (SendCmdClass) {
                            case 1:
                                ChangeAxisOrgginData[0] = (ChangeAxisOrgginData[0] + RobotSendData[0]*10);
                                ChangeAxisOrgginData[1] = ChangeAxisOrgginData[1]+0.0f;
                                ChangeAxisOrgginData[2] = ChangeAxisOrgginData[2]+0.0f;
                                ChangeAxisOrgginData[3] = ChangeAxisOrgginData[3]+0.0f;
                                ChangeAxisOrgginData[4] = ChangeAxisOrgginData[4]+0.0f;
                                ChangeAxisOrgginData[5] = ChangeAxisOrgginData[5]+0.0f;
                                for(int i = 0;i<10;i++){
                                    SetMotorOnCoordinateLost(ChangeAxisOrgginData[0]/1000,ChangeAxisOrgginData[1]/1000,ChangeAxisOrgginData[2]/1000,ChangeAxisOrgginData[3],ChangeAxisOrgginData[4],ChangeAxisOrgginData[5],RobotSpeedValue);
                                    delay_ms(10);
                                }

                                RobotDataStatusS[0] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 0;

                                break;
                            case 2:
                                ChangeAxisOrgginData[0] = ChangeAxisOrgginData[0]+0.0f;
                                ChangeAxisOrgginData[1] = (ChangeAxisOrgginData[1] + RobotSendData[1]*10);
                                ChangeAxisOrgginData[2] = ChangeAxisOrgginData[2]+0.0f;
                                ChangeAxisOrgginData[3] = ChangeAxisOrgginData[3]+0.0f;
                                ChangeAxisOrgginData[4] = ChangeAxisOrgginData[4]+0.0f;
                                ChangeAxisOrgginData[5] = ChangeAxisOrgginData[5]+0.0f;
                                for(int i = 0;i<10;i++){
                                    SetMotorOnCoordinateLost(ChangeAxisOrgginData[0]/1000,ChangeAxisOrgginData[1]/1000,ChangeAxisOrgginData[2]/1000,ChangeAxisOrgginData[3],ChangeAxisOrgginData[4],ChangeAxisOrgginData[5],RobotSpeedValue);
                                    delay_ms(10);
                                }

                                RobotDataStatusS[1] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 1;
                                break;
                            case 3:
                                ChangeAxisOrgginData[0] = ChangeAxisOrgginData[0]+0.0f;
                                ChangeAxisOrgginData[1] = ChangeAxisOrgginData[1]+0.0f;
                                ChangeAxisOrgginData[2] = (ChangeAxisOrgginData[2] + RobotSendData[2]*10);
                                ChangeAxisOrgginData[3] = ChangeAxisOrgginData[3]+0.0f;
                                ChangeAxisOrgginData[4] = ChangeAxisOrgginData[4]+0.0f;
                                ChangeAxisOrgginData[5] = ChangeAxisOrgginData[5]+0.0f;
                                for(int i = 0;i<10;i++){
                                    SetMotorOnCoordinateLost(ChangeAxisOrgginData[0]/1000,ChangeAxisOrgginData[1]/1000,ChangeAxisOrgginData[2]/1000,ChangeAxisOrgginData[3],ChangeAxisOrgginData[4],ChangeAxisOrgginData[5],RobotSpeedValue);
                                    delay_ms(10);
                                }

                                RobotDataStatusS[2] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 2;
                                break;
                            case 4:
                                ChangeAxisOrgginData[0] = ChangeAxisOrgginData[0]+0.0f;
                                ChangeAxisOrgginData[1] = ChangeAxisOrgginData[1]+0.0f;
                                ChangeAxisOrgginData[2] = ChangeAxisOrgginData[2]+0.0f;
                                ChangeAxisOrgginData[4] = ChangeAxisOrgginData[4]+0.0f;
                                ChangeAxisOrgginData[5] = ChangeAxisOrgginData[5]+0.0f;

                                ChangeAxisOrgginData[3] = (ChangeAxisOrgginData[3] + RobotSendData[3]/1)/1;
                                for(int i = 0;i<10;i++){
                                    SetMotorOnCoordinateLost(ChangeAxisOrgginData[0]/1000,ChangeAxisOrgginData[1]/1000,ChangeAxisOrgginData[2]/1000,ChangeAxisOrgginData[3],ChangeAxisOrgginData[4],ChangeAxisOrgginData[5],RobotSpeedValue);
                                    delay_ms(10);
                                }

                                RobotDataStatusS[3] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 3;
                                break;
                            case 5:
                                ChangeAxisOrgginData[4] = (ChangeAxisOrgginData[4] + RobotSendData[4]/1)/1;
                                ChangeAxisOrgginData[0] = ChangeAxisOrgginData[0]+0.0f;
                                ChangeAxisOrgginData[1] = ChangeAxisOrgginData[1]+0.0f;
                                ChangeAxisOrgginData[2] = ChangeAxisOrgginData[2]+0.0f;
                                ChangeAxisOrgginData[3] = ChangeAxisOrgginData[3]+0.0f;
                                ChangeAxisOrgginData[5] = ChangeAxisOrgginData[5]+0.0f;
                                for(int i = 0;i<10;i++){
                                    SetMotorOnCoordinateLost(ChangeAxisOrgginData[0]/1000,ChangeAxisOrgginData[1]/1000,ChangeAxisOrgginData[2]/1000,ChangeAxisOrgginData[3],ChangeAxisOrgginData[4],ChangeAxisOrgginData[5],RobotSpeedValue);
                                    delay_ms(10);
                                }

                                RobotDataStatusS[4] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 4;
                                break;
                            case 6:
                                ChangeAxisOrgginData[0] = ChangeAxisOrgginData[0]+0.0f;
                                ChangeAxisOrgginData[1] = ChangeAxisOrgginData[1]+0.0f;
                                ChangeAxisOrgginData[2] = ChangeAxisOrgginData[2]+0.0f;
                                ChangeAxisOrgginData[3] = ChangeAxisOrgginData[3]+0.0f;
                                ChangeAxisOrgginData[4] = ChangeAxisOrgginData[4]+0.0f;
                                ChangeAxisOrgginData[5] = ChangeAxisOrgginData[5] + RobotSendData[5];
                                for(int i = 0;i<10;i++){
                                    SetMotorOnCoordinateLost(ChangeAxisOrgginData[0]/1000,ChangeAxisOrgginData[1]/1000,ChangeAxisOrgginData[2]/1000,ChangeAxisOrgginData[3],ChangeAxisOrgginData[4],ChangeAxisOrgginData[5],RobotSpeedValue);
                                    delay_ms(10);
                                }

                                RobotDataStatusS[5] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 5;
                                break;

                            case 7:
                                ChangeAnale[0] =  ChangeAnale[0] + RobotSendData[6]/2;
                                ChangeAnale[1] =  ChangeAnale[1] + 0.0f;
                                ChangeAnale[2] =  ChangeAnale[2] + 0.0f;
                                ChangeAnale[3] =  ChangeAnale[3] + 0.0f;
                                ChangeAnale[4] =  ChangeAnale[4] + 0.0f;
                                ChangeAnale[5] =  ChangeAnale[5] + 0.0f;
                                for(int i = 0;i<10;i++){

                                    SetRobotFromAngleLosr(ChangeAnale[0],ChangeAnale[1],ChangeAnale[2],ChangeAnale[3],ChangeAnale[4],ChangeAnale[5],RobotSpeedValue);
                                    delay_ms(1);
                                }
                                RobotDataStatusS[6] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 6;
                                break;
                            case 8:

                                ChangeAnale[0] =  ChangeAnale[0] + 0.0f;
                                ChangeAnale[2] =  ChangeAnale[2] + 0.0f;
                                ChangeAnale[3] =  ChangeAnale[3] + 0.0f;
                                ChangeAnale[4] =  ChangeAnale[4] + 0.0f;
                                ChangeAnale[5] =  ChangeAnale[5] + 0.0f;
                                ChangeAnale[1] = ChangeAnale[1] + RobotSendData[7]/2;
                                for(int i = 0;i<10;i++){

                                    SetRobotFromAngleLosr(ChangeAnale[0],ChangeAnale[1],ChangeAnale[2],ChangeAnale[3],ChangeAnale[4],ChangeAnale[5],RobotSpeedValue);
                                    delay_ms(1);
                                }
                                RobotDataStatusS[7] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 7;
                                break;
                            case 9:

                                ChangeAnale[1] =  ChangeAnale[1] + 0.0f;
                                ChangeAnale[0] =  ChangeAnale[0] + 0.0f;
                                ChangeAnale[3] =  ChangeAnale[3] + 0.0f;
                                ChangeAnale[4] =  ChangeAnale[4] + 0.0f;
                                ChangeAnale[5] =  ChangeAnale[5] + 0.0f;
                                ChangeAnale[2] =  ChangeAnale[2] + RobotSendData[8]/2;
                                for(int i = 0;i<10;i++){

                                    SetRobotFromAngleLosr(ChangeAnale[0],ChangeAnale[1],ChangeAnale[2],ChangeAnale[3],ChangeAnale[4],ChangeAnale[5],RobotSpeedValue);
                                    delay_ms(1);
                                }
                                RobotDataStatusS[8] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 8;
                                break;
                            case 10:
                                ChangeAnale[1] =  ChangeAnale[1] + 0.0f;
                                ChangeAnale[2] =  ChangeAnale[2] + 0.0f;
                                ChangeAnale[0] =  ChangeAnale[0] + 0.0f;
                                ChangeAnale[4] =  ChangeAnale[4] + 0.0f;
                                ChangeAnale[5] =  ChangeAnale[5] + 0.0f;
                                ChangeAnale[3] =  ChangeAnale[3] + RobotSendData[9]/2;
                                for(int i = 0;i<10;i++){

                                    SetRobotFromAngleLosr(ChangeAnale[0],ChangeAnale[1],ChangeAnale[2],ChangeAnale[3],ChangeAnale[4],ChangeAnale[5],RobotSpeedValue);
                                    delay_ms(1);
                                }
                                RobotDataStatusS[9] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 9;
                                break;
                            case 11:

                                ChangeAnale[1] =  ChangeAnale[1] + 0.0f;
                                ChangeAnale[2] =  ChangeAnale[2] + 0.0f;
                                ChangeAnale[3] =  ChangeAnale[3] + 0.0f;
                                ChangeAnale[0] =  ChangeAnale[0] + 0.0f;
                                ChangeAnale[5] =  ChangeAnale[5] + 0.0f;
                                ChangeAnale[4] =  ChangeAnale[4] + RobotSendData[10]/2;
                                for(int i = 0;i<10;i++){

                                    SetRobotFromAngleLosr(ChangeAnale[0],ChangeAnale[1],ChangeAnale[2],ChangeAnale[3],ChangeAnale[4],ChangeAnale[5],RobotSpeedValue);
                                    delay_ms(1);
                                }
                                RobotDataStatusS[10] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 10;
                                break;
                            case 12:
                                ChangeAnale[1] =  ChangeAnale[1] + 0.0f;
                                ChangeAnale[2] =  ChangeAnale[2] + 0.0f;
                                ChangeAnale[3] =  ChangeAnale[3] + 0.0f;
                                ChangeAnale[4] =  ChangeAnale[4] + 0.0f;
                                ChangeAnale[0] =  ChangeAnale[0] + 0.0f;
                                ChangeAnale[5] =  ChangeAnale[5] + RobotSendData[11]/2;
                                for(int i = 0;i<10;i++){

                                    SetRobotFromAngleLosr(ChangeAnale[0],ChangeAnale[1],ChangeAnale[2],ChangeAnale[3],ChangeAnale[4],ChangeAnale[5],RobotSpeedValue);
                                    delay_ms(1);
                                }
                                RobotDataStatusS[11] = 1;
                                RobotDataStatusFlag = 1;
                                RobotDataSendIndex = 11;
                                break;

                            case 13:

                                SetRobotPause();
                                break;
                            case 14:
                                if (RobotReportLock == 0) {
                                    SetRobotWork();
                                    RobotReportLock = 0;

                                }

                                break;
                        }

                        SendCmdClass = 0;
                    }
                }
                if(SendFlah){
                    SendFlah = 0;
                    if(ContralMode == 1 ){
                        for(int i = 0;i<10;i++) {
                            SetRobotFromAngleLosr(0.0f, 0.0f, 0.0f,0.0f,0.0f,0.0f, RobotSpeedValue);
                            delay_ms(10);
                        }
                        for(int i = 0;i<6;i++){
                            ChangeAnale[i] = OriginAngle[i];
                        }
                    }
                    if(ContralMode == 0){
                        for(int i = 0;i<10;i++) {
                            SetMotorOnCoordinateLost(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,RobotSpeedValue);
                            delay_ms(10);
                        }
                        for(int i = 0;i<6;i++){
                            ChangeAxisOrgginData[i] = AxisOrgginData[i];
                        }
                    }
                }
            }

        }
        vTaskDelay(1);
    }
}
void Main(void){

    //xTaskCreate(vTask1,"LED1",128,NULL,1,NULL);

//xTaskCreate(vTask2,"LED2",128,NULL,1,NULL);
    xTaskCreate(System,"system",128,NULL,1,NULL);
    xTaskCreate(lcdentry,"lcd",512,NULL,1,NULL);
    xTaskCreate(keyscan,"key",128,NULL,1,NULL);
    xTaskCreate(ledentry,"led",128,NULL,1,NULL);
    xTaskCreate(xbeesend,"xbeesend",512,NULL,1,NULL);
    vTaskStartScheduler();

}